import panda_py
from panda_py import libfranka
import numpy as np

panda = panda_py.Panda("172.16.0.2")
hand = libfranka.Gripper("172.16.0.2")

panda.move_to_start()
cur_pose = panda.get_pose()
print("Current Pose:", cur_pose)
pose=panda.get_pose()

theta=np.pi/4  # 45 degrees

Rz=np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta), np.cos(theta), 0],
    [0, 0, 1]
    ])

pose[:3,:3]=Rz @ pose[:3,:3]

panda.move_to_pose(pose, dq_threshold=5e-4)
cur_pose = panda.get_pose()
print("Current Pose:", cur_pose)

